/********************************************************************************
 * Copyright 2009 The Robotics Group, The Maersk Mc-Kinney Moller Institute,
 * Faculty of Engineering, University of Southern Denmark
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 ********************************************************************************/

#include "DistanceUtil.hpp"

#include <rw/math/Math.hpp>

using namespace rw::math;
using namespace rw::geometry;

double DistanceUtil::distanceLineLineSqr (const rw::math::Vector3D<>& p1,
                                          const rw::math::Vector3D<>& p2,
                                          const rw::math::Vector3D<>& q1,
                                          const rw::math::Vector3D<>& q2)
{
    Vector3D<> c1, c2;
    double s, t;
    const double EPSILON = 0.0000001;
    Vector3D<> d1        = p2 - p1;
    Vector3D<> d2        = q2 - q1;
    Vector3D<> r         = p1 - q1;

    double a = dot (d1, d1);
    double e = dot (d2, d2);
    double f = dot (d2, r);

    if (a <= EPSILON && e <= EPSILON) {
        // both segments degenerate into points
        s = t = 0.0;
        c1    = p1;
        c2    = q1;
        return dot (c1 - c2, c1 - c2);
    }

    if (a <= EPSILON) {
        s = 0.0;
        t = f / e;
        t = Math::clamp (t, 0.0, 1.0);
    }
    else {
        double c = dot (d1, r);
        if (e <= EPSILON) {
            t = 0.0;
            s = Math::clamp (-c / a, 0.0, 1.0);
        }
        else {
            double b     = dot (d1, d2);
            double denom = a * e - b * b;

            if (denom != 0.0) {
                s = Math::clamp ((b * f - c * e) / denom, 0.0, 1.0);
            }
            else {
                s = 0.0;
            }

            t = (b * s + f) / e;

            if (t < 0.0) {
                t = 0.0;
                s = Math::clamp (-c / a, 0.0, 1.0);
            }
            else if (t > 1.0) {
                t = 1.0;
                s = Math::clamp ((b - c) / a, 0.0, 1.0);
            }
        }
    }

    c1 = p1 + d1 * s;
    c2 = q1 + d2 * t;
    return dot (c1 - c2, c1 - c2);
}

double DistanceUtil::distanceLineLine (const Vector3D<>& p1, const Vector3D<>& p2,
                                       const Vector3D<>& q1, const Vector3D<>& q2)
{
    return sqrt (distanceLineLineSqr (p1, p2, q1, q2));
}
